#include <QtGui/QApplication>
#include "mainwindow.h"
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <vector>
#include <cmath>
#include <QMutex>
#include <iostream>
#include "dialog.h"
#include "mainwindow.h"

#define pi_define 3.141592654

using namespace std;
typedef enum
{
    waitingForParameters,
    moveDCmotor,
    moveStepperMotor,
    moveGrippers,
    ready,
    done,
} StateT;
static StateT CurrentState = waitingForParameters;

/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
MainWindow *guiWindow;
Dialog     *userWindow;
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////////////////////
class MyThread_robot_thread : public QThread
{
    public:
        virtual void run();
};

void MyThread_robot_thread::run()
{
    static StateT NextState;
    printf("Entering robot thread. \n");
    while(guiWindow->start_or_end_program_flag == 1)
    {
       /* guiWindow->DC_motor_0->move_motor();
        float temp = guiWindow->DC_motor_0->my_DC_phidget_motor->commandVoltage;
        float temp2 = guiWindow->DC_motor_0->my_DC_phidget_motor->kp;
        float temp3 = guiWindow->DC_motor_0->my_DC_phidget_motor->kv;
        float temp4 = guiWindow->DC_motor_0->my_DC_phidget_motor->ki;
        printf("voltage: %f   %f   %f   %f\n", temp, temp2, temp3, temp4);*/

        ////////////////
        /*
        for(int i = 0; i < guiWindow->servo_board->num_servos; i++)
        {
            guiWindow->servo_board->updatePosition(i);
            guiWindow->servo_board->move_motor(i, guiWindow->servo_board->goal_pos[i]);
        }
        */
        ////////////////

        //Run a StateMachine here...
        switch ( CurrentState )
        {
            case waitingForParameters :
                if (userWindow->user_enabled_flag == true)
                {
                    NextState = moveDCmotor;
                }
            break;

            case moveDCmotor:
            {
                printf("moving dc motor........... \n\r");
            }
            break;

         } // end switch on state
         CurrentState = NextState;
    }

    printf("Exiting robot thread. \n");
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////

int main(int argc, char *argv[])
{
    cout << "Program entered." << endl;

    QApplication app(argc, argv);

    guiWindow = new MainWindow();
    guiWindow->setGeometry(50,50,1300,600);
    guiWindow->setAutoFillBackground(true);
    guiWindow->show();

    userWindow = new Dialog();
    userWindow->show();

    /////////////////////////////////////////////////////////////////////////////////////////////////////////
    MyThread_robot_thread robot_thread;
    robot_thread.start(QThread::TimeCriticalPriority);
    /////////////////////////////////////////////////////////////////////////////////////////////////////////

    
    return app.exec(); //runs qt program NECESSARY waits until the gui the thread is finished, which is when all the windows are closed
    //keep doing the gui until thread killed
    robot_thread.wait();
}
